
#include "cvf_Histogram.h"

#define USE_INTEGRAL 0

CRectHistogram::CParameters::CParameters()
{
	Clear();
}

void CRectHistogram::CParameters::Clear()
{
	myInit(1, 1, 1, 1, 1, 0.0, "", 0.0, 1.0);
}

void CRectHistogram::CParameters::myInit(istream& inStream)
{
	int ba,wwc,hwc,wbc,hbc;
	double ee, hysth, wtscale;
	string nm;
	inStream>>ba>>wwc>>hwc>>wbc>>hbc>>wtscale>>nm>>hysth>>ee;
	myInit(ba,wwc,hwc,wbc,hbc,wtscale,nm,hysth,ee);
}

void CRectHistogram::CParameters::myInit(
	int BinAmount,
	int wWinCel, int hWinCel,
	int wBlkCel, int hBlkCel,
	double WeightScale,
	string strNormalizer, double hysTheshold,
	double e
	)
{
	this->BinAmount = BinAmount;
	this->e = e;

	this->WeightScale = WeightScale;

	normalizer = L2_NORM; hysth = 0.0;
	CDataStructureTools::ToLowerCase(strNormalizer);
	if (strNormalizer == "l2-hys")
	{
		normalizer = L2_HYS;
		hysth = hysTheshold;
	}

	this->wWinCel = wWinCel;
	this->hWinCel = hWinCel;
	this->wBlkCel = wBlkCel;
	this->hBlkCel = hBlkCel;

	wWinBlk = wWinCel - wBlkCel + 1;
	hWinBlk = hWinCel - hBlkCel + 1;

	aWinCel = wWinCel * hWinCel;
	aBlkCel = wBlkCel * hBlkCel;
	aWinBlk = wWinBlk * hWinBlk;

	lenBlkDescriptor = aBlkCel * BinAmount;
	lenWinDescriptor = aWinBlk * lenBlkDescriptor;
}

CRectHistogram::CRectHistogram()
{
	W = H = WH = 0;
	matWeight.Clear();
	matValue.Clear();

	WindowDescriptor.myInit();
	//int i;
	//WindowDescriptorList.Resize(1);
	//for ( i = 0; i < WindowDescriptorList.Size; ++i )
	//{
	//	CWindowDescriptor* pWD;
	//	pWD->myInit();
	//}

	Clear();
}

void CRectHistogram::SetData(int W, int H, const double* matWeight, const double* matValue)
{
	int i;
	double* pW;
	double* pV;
	const double* qW;
	const double* qV;

	this->W = W;
	this->H = H;
	this->WH = W * H;
	this->matWeight.Resize(WH);
	this->matValue.Resize(WH);

	pW = this->matWeight.pElement;
	qW = matWeight;
	pV = this->matValue.pElement;
	qV = matValue;

	if (matWeight)
		for ( i = 0; i < WH; ++i )
			*(pW++) = *(qW++);
	else
		for ( i = 0; i < WH; ++i )
			*(pW++) = 1.0;
	if (matValue)
		for ( i = 0; i < WH; ++i )
			*(pV++) = *(qV++);
	else
		for ( i = 0; i < WH; ++i )
			*(pV++) = 0.0;

	Clear();

	WindowDescriptor.SetImage(this);
}

void CRectHistogram::SetData(int W, int H, const double* matValue)
{
	SetData(W, H, NULL, matValue);
}

void CRectHistogram::Clear()
{
	WindowDescriptor.ClearCalculationHistory();
	WindowDescriptor.SetImage(NULL);
}

void CRectHistogram::WorkOnBlock(
	const CRectBox<int>& Box,
	int xBlk, int yBlk,
	double* dstBlockDescriptor
	)
{
	WindowDescriptor.SetWindow(Box);
	WindowDescriptor.WorkOnBlock(xBlk, yBlk, dstBlockDescriptor);
}

CRectHistogram::CWindowDescriptor::CWindowDescriptor()
{
	myInit();
}

void CRectHistogram::CWindowDescriptor::myInit()
{
	pSource = NULL;
	RectHist_Parameters.Clear();
	HistogramParameter.myInit(RectHist_Parameters.BinAmount, false, 0.0, 1.0);
	ClearCalculationHistory();

	_BlkQuery = _BlkCalc = 0;
	_CelQuery = _CelCalc = 0;
	//_CelQuery = _CelCalc = -1;
}

void CRectHistogram::CWindowDescriptor::ClearCalculationHistory()
{
	BlkCache.Resize(pSource ? pSource->WH : 1);
	BlkCache.Fill(-1);
	BlkData.Clear();
	_fIsImagePreprocessed = false;

	W = (pSource==NULL ? 0 : pSource->W);
	H = (pSource==NULL ? 0 : pSource->H);
	WH = W * H;

	Box.x = Box.y = -1;
	Box.w = Box.h = 0;
}

void CRectHistogram::CWindowDescriptor::SetParameters(const CParameters& RectHist_Parameters, double ValueBoundA, double ValueBoundB, bool fAreBinsWrapped)
{
	this->RectHist_Parameters = RectHist_Parameters;
	HistogramParameter.myInit(RectHist_Parameters.BinAmount, fAreBinsWrapped, ValueBoundA, ValueBoundB);
	ClearCalculationHistory();
}

void CRectHistogram::CWindowDescriptor::SetImage(CRectHistogram* pSource)
{
	this->pSource = pSource;
	ClearCalculationHistory();
}

void CRectHistogram::CWindowDescriptor::SetWindow(const CRectBox<int>& Box)
{
	CRectBox<int> tBox;
	tBox.x = (Box.x<0 ? 0 : (Box.x<W ? Box.x : W-1));
	tBox.y = (Box.y<0 ? 0 : (Box.y<H ? Box.y : H-1));
	tBox.w = (Box.w<1 ? 1 : (Box.w<=W-tBox.x ? Box.w : W-tBox.x));
	tBox.h = (Box.h<1 ? 1 : (Box.h<=H-tBox.y ? Box.h : H-tBox.y));
	if (false
		|| tBox.x!=this->Box.x
		|| tBox.y!=this->Box.y
		|| tBox.w!=this->Box.w
		|| tBox.h!=this->Box.h
		)
	{
		this->Box = tBox;
	}
}

void CRectHistogram::CWindowDescriptor::_PreprocessImage()
{
	if (_fIsImagePreprocessed) return;
	_fIsImagePreprocessed = true;
	if (pSource == NULL) return;

#if USE_INTEGRAL
	_Real_PreprocessImage_B();
#else
	_Real_PreprocessImage_A();
#endif

	int z = (int)BlkData.Size;
	int zz = (int)(WH / (1.0 * Box.w * Box.h / RectHist_Parameters.wBlkCel / RectHist_Parameters.hBlkCel)) * RectHist_Parameters.lenBlkDescriptor;
	if (zz>z)
	{
		BlkData.Resize(zz, 0.0);
		BlkData.Resize(z);
	}
}

void CRectHistogram::CWindowDescriptor::WorkOnBlock(
	int xBlk, int yBlk,
	double* dstBlockDescriptor
	)
{
	int ix,iy,j,data_pos,jx,jy,o1,o2,tx,ty;
	double* pchd;
	const double* qchd;
	double t;

	_PreprocessImage();

	double wCel = 1.0 * Box.w / RectHist_Parameters.wWinCel;
	double hCel = 1.0 * Box.h / RectHist_Parameters.hWinCel;
	xBlk = (xBlk<0 ? 0 : (xBlk<RectHist_Parameters.wWinBlk ? xBlk : RectHist_Parameters.wWinBlk-1));
	yBlk = (yBlk<0 ? 0 : (yBlk<RectHist_Parameters.hWinBlk ? yBlk : RectHist_Parameters.hWinBlk-1));
	double gx = Box.x + wCel * (xBlk + RectHist_Parameters.wBlkCel / 2.0);
	double gy = Box.y + hCel * (yBlk + RectHist_Parameters.hBlkCel / 2.0);

	ix = (int)(gx + .5); ix = (ix<0 ? 0 : (ix<W ? ix : W-1));
	iy = (int)(gy + .5); iy = (iy<0 ? 0 : (iy<H ? iy : H-1));
	o1 = 0; for ( jy = -o1; jy <= +o1; ++jy )
	{
		ty = iy + jy; if (ty<0 || ty>=H) continue;
		o2 = o1 - abs(jy); for ( jx = -o2; jx <= +o2; ++jx )
		{
			tx = ix + jx; if (tx<0 || tx>=W) continue;
			j = tx + ty * W; data_pos = BlkCache.pElement[j];
			if (data_pos >= 0) break;
		}
		if (jx <= +o2) break;
	}
	++_BlkQuery;
	if (data_pos < 0)
	{
		++_BlkCalc;
		j = ix + iy * W; BlkCache.pElement[j] = data_pos = (int)BlkData.Size;
		BlkData.Resize(BlkData.Size + RectHist_Parameters.lenBlkDescriptor, 0.0);
		pchd = BlkData.pElement + data_pos;
		for ( j = RectHist_Parameters.lenBlkDescriptor; j > 0; --j )
			*(pchd++) = 0.0;

#if USE_INTEGRAL
		_CalcBlockDescriptor_B(BlkData.pElement + data_pos, Box.x + wCel * xBlk, Box.y + hCel * yBlk, wCel, hCel);
#else
		_CalcBlockDescriptor_A(BlkData.pElement + data_pos, Box.x + wCel * xBlk, Box.y + hCel * yBlk, wCel, hCel);
#endif

		// Do Normalization
		if (RectHist_Parameters.normalizer==CParameters::L2_NORM || RectHist_Parameters.normalizer==CParameters::L2_HYS)
			CMyMath::L2_Normalize(BlkData.pElement + data_pos, RectHist_Parameters.lenBlkDescriptor, RectHist_Parameters.e / RectHist_Parameters.lenBlkDescriptor);
		if (RectHist_Parameters.normalizer==CParameters::L2_HYS)
		{
			t = RectHist_Parameters.hysth;
			pchd = BlkData.pElement + data_pos;
			for ( j = RectHist_Parameters.lenBlkDescriptor; j > 0; --j )
			{
				if (*pchd > t)
					*pchd = t;
				++pchd;
			}
			CMyMath::L2_Normalize(BlkData.pElement + data_pos, RectHist_Parameters.lenBlkDescriptor, 0.01 / RectHist_Parameters.lenBlkDescriptor);
		}
	}
	qchd = BlkData.pElement + data_pos;
	pchd = dstBlockDescriptor;
	for ( j = RectHist_Parameters.lenBlkDescriptor; j > 0; --j )
		*(pchd++) = *(qchd++);
}

void CRectHistogram::CWindowDescriptor::_Real_PreprocessImage_A()
{
	matW_A.Resize(WH);
	matV_A.Resize(WH);
	matW_B.Resize(WH);
	matV_B.Resize(WH);

	int i,o1,o2;
	double tw,tv,tt;
	const double* pW = pSource->matWeight.pElement;
	const double* pV = pSource->matValue.pElement;
	for ( i = 0; i < WH; ++i )
	{
		tw = *(pW++);
		tt = *(pV++);
		tv = HistogramParameter.SelectBin_Bounding(o1,o2,tt);
		matW_A.pElement[i] = tw * (1.0 - tv);
		matV_A.pElement[i] = o1;
		matW_B.pElement[i] = tw * tv;
		matV_B.pElement[i] = o2;
	}
}

void CRectHistogram::CWindowDescriptor::_CalcBlockDescriptor_A(double* DstDescriptor, double xBase, double yBase, double wCel, double hCel)
{
	int xMin = (int)(xBase + wCel * (-0.5));
	int xMax = (int)(xBase + wCel * (+0.5 + RectHist_Parameters.wBlkCel));
	int yMin = (int)(yBase + hCel * (-0.5));
	int yMax = (int)(yBase + hCel * (+0.5 + RectHist_Parameters.hBlkCel));

	int jx, jy, tx, ty, o1, o2;
	double gx = xBase + wCel * 0.5 * RectHist_Parameters.wBlkCel;
	double gy = yBase + hCel * 0.5 * RectHist_Parameters.hBlkCel;

	double kSx = RectHist_Parameters.WeightScale * wCel * RectHist_Parameters.wBlkCel / 2.0;
	double kSy = RectHist_Parameters.WeightScale * hCel * RectHist_Parameters.hBlkCel / 2.0;
	double skx, sky, final_weight;
	CSimpleTypeArray<double> kCx, kCy;

	kCx.Resize(xMax - xMin + 1); skx = 0.0;
	for ( jx = xMin; jx <= xMax; ++jx ) skx += kCx.pElement[jx - xMin] = CMyMath::gaussian((double)jx - gx, 0.0, kSx);
	kCy.Resize(yMax - yMin + 1); sky = 0.0;
	for ( jy = yMin; jy <= yMax; ++jy ) sky += kCy.pElement[jy - yMin] = CMyMath::gaussian(0.0, (double)jy - gy, kSy);

	final_weight = (RectHist_Parameters.wBlkCel * RectHist_Parameters.hBlkCel) / (skx * sky);

	const double* pkCx;
	const double* pWA;
	const int* pVA;
	const double* pWB;
	const int* pVB;
	double dgy,dgxy,w1,w2;
	double cy2,cx2,ux1,ux2,uy1,uy2;
	int ey2,ex2;
	double* p11, *p12, *p21, *p22;

	cy2 = yBase + hCel * 0.5 - 0.5; ey2 = 0;
	for ( jy = yMin; jy <= yMax; ++jy )
	{
		ty = (jy<0 ? 0 : (jy<H ? jy : H-1));
		pWA = matW_A.pElement + ty * W;
		pVA = matV_A.pElement + ty * W;
		pWB = matW_B.pElement + ty * W;
		pVB = matV_B.pElement + ty * W;
		pkCx = kCx.pElement;
		
		dgy = kCy.pElement[jy - yMin] * final_weight;

		if (jy > cy2) { cy2 += hCel; ++ey2; }
		uy1 = cy2 - jy; uy2 = hCel - uy1;

		p11 = NULL; p12 = NULL;
		if (ey2<1 || ey2-1>=RectHist_Parameters.hBlkCel)
			p21 = NULL;
		else
			p21 = DstDescriptor + RectHist_Parameters.BinAmount * (ey2-1) * RectHist_Parameters.wBlkCel;
		if (ey2>=RectHist_Parameters.hBlkCel)
			p22 = NULL;
		else
			p22 = DstDescriptor + RectHist_Parameters.BinAmount * (ey2-0) * RectHist_Parameters.wBlkCel;

		cx2 = xBase + wCel * 0.5 - 0.5; ex2 = 0;

		for ( jx = xMin; jx <= xMax; ++jx )
		{
			tx = (jx<0 ? 0 : (jx<W ? jx : W-1));
			dgxy = dgy * (*(pkCx++));

			w1 = pWA[tx]; o1 = pVA[tx]; w1 *= dgxy;
			w2 = pWB[tx]; o2 = pVB[tx]; w2 *= dgxy;

			if (jx > cx2)
			{
				cx2 += wCel; ++ex2;
				p11 = p21; if (p21!=NULL && ex2<RectHist_Parameters.wBlkCel) p21 += RectHist_Parameters.BinAmount; else p21 = NULL;
				p12 = p22; if (p22!=NULL && ex2<RectHist_Parameters.wBlkCel) p22 += RectHist_Parameters.BinAmount; else p22 = NULL;
			}
			ux1 = cx2 - jx; ux2 = wCel - ux1;

			if (p11 != NULL)
			{
				p11[o1] += w1 * ux1 * uy1;
				p11[o2] += w2 * ux1 * uy1;
			}
			if (p21 != NULL)
			{
				p21[o1] += w1 * ux2 * uy1;
				p21[o2] += w2 * ux2 * uy1;
			}
			if (p22 != NULL)
			{
				p22[o1] += w1 * ux2 * uy2;
				p22[o2] += w2 * ux2 * uy2;
			}
			if (p12 != NULL)
			{
				p12[o1] += w1 * ux1 * uy2;
				p12[o2] += w2 * ux1 * uy2;
			}
		}
	}
}

void CRectHistogram::CWindowDescriptor::_Real_PreprocessImage_B()
{
	IntegralHistogramMap.Resize(WH * RectHist_Parameters.BinAmount);
	IntegralHistogramMap.Fill(0.0);
	int i, o1, o2;
	double tw, tv;
	double* pd = IntegralHistogramMap.pElement;
	const double* qW = pSource->matWeight.pElement;
	const double* qV = pSource->matValue.pElement;
	for ( i = 0; i < WH; ++i )
	{
		tw = *(qW++);
		tv = HistogramParameter.SelectBin_Bounding(o1,o2,*(qV++));
		pd[o1] += tw * (1.0 - tv);
		pd[o2] += tw * tv;
		pd += RectHist_Parameters.BinAmount;
	}
	for ( o2 = 0; o2 < H; ++o2 )
	{
		for ( o1 = 1; o1 < W; ++ o1)
		{
			pd = IntegralHistogramMap.pElement + RectHist_Parameters.BinAmount * (o1 + o2 * W);
			qW = pd - RectHist_Parameters.BinAmount;
			for ( i = RectHist_Parameters.BinAmount; i > 0; --i )
				*(pd++) += *(qW++);
		}
		if (o2 == 0) continue;
		pd = IntegralHistogramMap.pElement + RectHist_Parameters.BinAmount * W * o2;
		qW = pd - W * RectHist_Parameters.BinAmount;
		for ( i = W * RectHist_Parameters.BinAmount; i > 0; --i )
			*(pd++) += *(qW++);
	}

	//CExternalTools::ConsoleTools::ClearCurrentLine();
	//cout.precision(3);
	//cout<<fixed<<1.0*_CelQuery/_CelCalc<<" "<<_CelCalc<<" "<<_CelQuery;
	//_CelQuery = _CelCalc = 0;

	CellCache.clear();
	CellData.Clear();
}

void CRectHistogram::CWindowDescriptor::_CalcBlockDescriptor_B(double* DstDescriptor, double xBase, double yBase, double wCel, double hCel)
{
	int t_BinAmount = RectHist_Parameters.BinAmount;
	int t_wBlkCel = RectHist_Parameters.wBlkCel;
	int t_hBlkCel = RectHist_Parameters.hBlkCel;

	xBase -= 1.0; yBase -= 1.0;

	if (0)
	{
		int ix, iy;
		for ( iy = 0; iy < t_hBlkCel; ++iy )
			for ( ix = 0; ix < t_wBlkCel; ++ix )
				_CalcCellHistogram_B(
					DstDescriptor + t_BinAmount * (ix + iy * t_wBlkCel)
					, (int)(xBase + wCel * ix + 0.5)
					, (int)(yBase + wCel * iy + 0.5)
					, (int)(xBase + wCel * ix + 0.5 + wCel)
					, (int)(yBase + wCel * iy + 0.5 + hCel)
					);
	}
	else
	{
		int tx, ty;
		int ix, iy, j;
		double* pd;
		double* ppd;
		const double* qd;
		const double* qqd;
		for ( iy = t_hBlkCel; iy >= 0; --iy )
		{
			ty = (int)(yBase + hCel * iy + 0.5);
			if (ty<0) continue;
			if (ty>=H) ty = H-1;
			for ( ix = t_wBlkCel; ix >= 0; --ix )
			{
				tx = (int)(xBase + wCel * ix + 0.5);
				if (tx<0) continue;
				if (tx>=W) tx = W-1;
				qd = IntegralHistogramMap.pElement + t_BinAmount * (tx + ty * W);

				pd = DstDescriptor + t_BinAmount * (ix + iy * t_wBlkCel);
				if (ix>0 && iy>0)
				{
					qqd = qd; ppd = pd - t_BinAmount * (1 + t_wBlkCel);
					for ( j = 0; j < t_BinAmount; ++j )
						*(ppd++) += *(qqd++);
				}
				if (ix<t_wBlkCel && iy>0)
				{
					qqd = qd; ppd = pd - t_BinAmount * t_wBlkCel;
					for ( j = 0; j < t_BinAmount; ++j )
						*(ppd++) -= *(qqd++);
				}
				if (ix>0 && iy<t_hBlkCel)
				{
					qqd = qd; ppd = pd - t_BinAmount;
					for ( j = 0; j < t_BinAmount; ++j )
						*(ppd++) -= *(qqd++);
				}
				if (ix<t_wBlkCel && iy<t_hBlkCel)
				{
					qqd = qd; ppd = pd;
					for ( j = 0; j < t_BinAmount; ++j )
						*(ppd++) += *(qqd++);
				}
				qd = qd;
			}
		}
	}
}

void CRectHistogram::CWindowDescriptor::_CalcCellHistogram_B(double* DstHistogram, int xL, int yL, int xH, int yH)
{
	int rp,i,tba;
	double* pd;
	const double* qd;

	++_CelQuery;
	tba = RectHist_Parameters.BinAmount;
	if (xH >= W) xH = W - 1;
	if (yH >= H) yH = H - 1;
	if (xL>=W || xH<0 || xL>=xH || yL>=H || yH<0 || yL>=yH)
	{
		pd = DstHistogram;
		for ( i = 0; i < tba; ++i ) *(pd++) = 0.0;
		return;
	}

	i = xH + W * (yH + H * (xL + W * yL));
	map<int, int>::const_iterator itr = CellCache.find(i);
	if (itr == CellCache.end())
	{
		++_CelCalc;
		CellCache[i] = rp = (int)CellData.Size;
		CellData.Resize(rp + tba, 0.0);
		if (xL>=0 && yL>=0)
		{
			pd = CellData.pElement + rp; qd = IntegralHistogramMap.pElement + tba * (xL + yL * W);
			for ( i = 0; i < tba; ++i ) *(pd++) += *(qd++);
		}
		if (xL>=0)
		{
			pd = CellData.pElement + rp; qd = IntegralHistogramMap.pElement + tba * (xL + yH * W);
			for ( i = 0; i < tba; ++i ) *(pd++) -= *(qd++);
		}
		if (yL>=0)
		{
			pd = CellData.pElement + rp; qd = IntegralHistogramMap.pElement + tba * (xH + yL * W);
			for ( i = 0; i < tba; ++i ) *(pd++) -= *(qd++);
		}
		if (1)
		{
			pd = CellData.pElement + rp; qd = IntegralHistogramMap.pElement + tba * (xH + yH * W);
			for ( i = 0; i < tba; ++i ) *(pd++) += *(qd++);
		}
	}
	else
		rp = itr->second;

	pd = DstHistogram; qd = CellData.pElement + rp;
	for ( i = 0; i < tba; ++i ) *(pd++) = *(qd++);
}

